import rclpy
from rclpy.node import Node
from topic_msg.msg import CatStatus
from topic_msg.msg import CatHearing


class Catbrain(Node):
    def __init__(self, name):
        super().__init__(name)
        self._status = CatStatus.CAT_ACTIVE

        self._hearing_subscriper = self.create_subscription(
            CatHearing, "cat_hearing", self._rcv_hearing_callback, 10
        )

        self._status_publisher = self.create_publisher(CatStatus, "cat_status", 10)

    def __del__(self):
        self._hearing_subscriper.destroy()
        self._status_publisher.destroy()

    def _rcv_hearing_callback(self, msg):
        # self.get_logger().info(f"msg:{msg.cmd}")
        if msg.cmd == CatHearing.DONT_MOVE_1 or msg.cmd == CatHearing.DONT_MOVE_2:
            if self._status == CatStatus.CAT_ACTIVE:
                self._status = CatStatus.CAT_DONT_MOVE
            elif self._status == CatStatus.CAT_DONT_ANOUNCE:
                self._status = CatStatus.CAT_STOP
            else:
                pass

        elif msg.cmd == CatHearing.SHUT_DOWN_1 or msg.cmd == CatHearing.SHUT_DOWN_2:
            if self._status == CatStatus.CAT_ACTIVE:
                self._status = CatStatus.CAT_DONT_ANOUNCE
            elif self._status == CatStatus.CAT_DONT_MOVE:
                self._status = CatStatus.CAT_STOP
            else:
                pass

        elif msg.cmd == CatHearing.ACTIVE_1 or msg.cmd == CatHearing.ACTIVE_2:
            if self._status == CatStatus.CAT_STOP:
                self._status = CatStatus.CAT_DONT_ANOUNCE
            elif self._status == CatStatus.CAT_DONT_MOVE:
                self._status = CatStatus.CAT_ACTIVE
            else:
                pass

        elif msg.cmd == CatHearing.PHONATION_1 or msg.cmd == CatHearing.PHONATION_2:
            if self._status == CatStatus.CAT_STOP:
                self._status = CatStatus.CAT_DONT_MOVE
            elif self._status == CatStatus.CAT_DONT_ANOUNCE:
                self._status = CatStatus.CAT_ACTIVE
            else:
                pass
        elif msg.cmd == CatHearing.POWER_OFF:
            pass
        else:
            pass

        msg = CatStatus()
        msg.status = self._status
        self._status_publisher.publish(msg)


def main():
    rclpy.init()
    node = Catbrain("cat_brain")
    rclpy.spin(node)
    rclpy.shutdown()
